#include <robot/pacohead/cheadpose.h>

#include <algorithm>

namespace buola { namespace robot { namespace pacohead {

const double EYE_POS_Y=100.0f;
const double EYE_POS_Z=174.5f;
const double BASELINE=93.0f;

void CHeadPose::InitHeadToCamerasMatrix(mat::CMat34d &pM)
{
    //neck pitch
    pM=mat::rotation_x_3d(mJoints[JOINT_NECK_PITCH]);
    
    //neck roll
    pM=mat::rotation_y_3d(-mJoints[JOINT_NECK_ROLL])*pM;
    
    //neck yaw
    pM=mat::rotation_z_3d(mJoints[JOINT_NECK_YAW])*pM;
    
    //eye translation
    pM=mat::translation_3d(-mat::CVec3d(0,EYE_POS_Y,EYE_POS_Z))*pM;
    
    //eye tilt
    pM=mat::rotation_x_3d(-mJoints[JOINT_EYES_TILT])*pM;

    //switch y and z
    pM=mat::CMat34d{{1,0,0,0},{0,0,-1,0},{0,1,0,0}}*pM;
}

void CHeadPose::InitCamerasToHeadMatrix(mat::CMat34d &pM)
{
    //switch y and z
    pM=mat::CMat34d{{1,0,0,0},{0,0,-1,0},{0,1,0,0}};

    //eye tilt
    pM=mat::rotation_x_3d(mJoints[JOINT_EYES_TILT])*pM;

    //eye translation
    pM=mat::translation_3d(mat::CVec3d(0,EYE_POS_Y,EYE_POS_Z))*pM;
    
    //neck yaw
    pM=mat::rotation_z_3d(-mJoints[JOINT_NECK_YAW])*pM;
    
    //neck roll
    pM=mat::rotation_y_3d(mJoints[JOINT_NECK_ROLL])*pM;
    
    //neck pitch
    pM=mat::rotation_x_3d(-mJoints[JOINT_NECK_PITCH])*pM;
}

mat::CVec3d CHeadPose::HeadToCameras(const mat::CVec3d &pHead)
{
    mat::CMat34d lM;
    
    InitHeadToCamerasMatrix(lM);
    
    return lM*pHead;
}

mat::CVec3d CHeadPose::CamerasToHead(const mat::CVec3d &pCam)
{
    mat::CMat34d lM;
    
    InitCamerasToHeadMatrix(lM);
    
    return lM*pCam;
}

double CHeadPose::GetLookAtDist()
{
    double lTan=std::abs(tan(mJoints[5]));
    if(lTan<=0.001)
        lTan=0.001;
        
    return (BASELINE/2.0)/lTan;
}

double CHeadPose::InvKinematics(const mat::CVec3d &pV,int pIter)
{
    double lLen=sqrt(pV.x()*pV.x()+(pV.y()-EYE_POS_Y)*(pV.y()-EYE_POS_Y));

    mJoints[JOINT_EYES_TILT]=atan2(pV.z()-EYE_POS_Z,lLen)*0.5;

    double lLO2=pV.y()*pV.y()+pV.z()*pV.z()-EYE_POS_Z*EYE_POS_Z;
    double lLO=(lLO2>0.0?sqrt(lLO2):0.0);

    mJoints[JOINT_NECK_YAW]=atan2(pV.x(),lLO);
    mJoints[JOINT_NECK_ROLL]=0.0;
    mJoints[JOINT_NECK_PITCH]=0.0;

    mat::CMat34d lJacobian;
    mat::CVec3d lErrors;

    for(int i=0;i<pIter;i++)
    {
        ComputeJacobian(lErrors,pV,lJacobian);
        mat::CVec3d lDelta;
        SolveDeltaAngles(lErrors,lJacobian,lDelta);

        mJoints[JOINT_NECK_YAW]-=lDelta.x();
        mJoints[JOINT_NECK_ROLL]-=lDelta.y();
        mJoints[JOINT_NECK_PITCH]-=lDelta.z();
    }

    ComputeErrors(pV,lErrors);

    mat::CVec3d lNose(0,0,0);

    mat::CMat34d lCamToHead;
    
    InitCamerasToHeadMatrix(lCamToHead);

    mat::CVec3d lPos=lCamToHead*lNose;
    mat::CVec3d lNosePos=lPos;

    lNose.z()=1000.0;
    
    mat::CVec3d lDir=lCamToHead*lNose-lPos;

    lDir/=norm(lDir);

    lPos-=pV;

    //at kth (reverse in karlsruhe)
    mJoints[JOINT_EYES_RIGHT]=atan2(BASELINE/2.0,norm(lPos));
    mJoints[JOINT_EYES_LEFT]=-mJoints[JOINT_EYES_RIGHT];
    
    lPos=lNosePos+lDir*norm(lPos);

    msg_info() << "buola error:" << lErrors << "\n";

    return norm(lErrors);
}

void CHeadPose::Normalize()
{
    for(int i=0;i<7;i++)
    {
        mJoints[i]-=M_PI*round(mJoints[i]/M_PI);
    }
}

void CHeadPose::Limit(const CHeadPose &pMin,const CHeadPose &pMax,double pFactor)
{
    for(int i=0;i<7;i++)
    {
        mJoints[i]=std::max(pMin.mJoints[i]*pFactor,std::min(mJoints[i],pMax.mJoints[i]*pFactor));
    }
}

void CHeadPose::TestZero(double pE)
{
    for(int i=0;i<7;i++)
    {
        if(std::abs(mJoints[i])<pE)
            mJoints[i]=0.0;
    }
}

void CHeadPose::ComputeErrors(const mat::CVec3d &pTarget,
                            mat::CVec3d &pError)
{
    mat::CVec3d lCamera=HeadToCameras(pTarget);

    double lLen=std::max(1.0,norm(lCamera));
    
    pError.x()=lCamera.x()/lLen;
    pError.y()=lCamera.y()/lLen;

    mat::CVec3d lUp(pTarget.x(),pTarget.y(),pTarget.z()+EYE_POS_Y);
    mat::CVec3d lCameraUp=HeadToCameras(lUp)-lCamera;
    
    lLen = std::max(1.0,sqrt(lCameraUp.x()*lCameraUp.x() + lCameraUp.y()*lCameraUp.y()));

    pError.z()=lCameraUp.x()/lLen;
}

void CHeadPose::ComputeJacobianColumn(
        const mat::CVec3d &pError,const mat::CVec3d &pTarget,
                      mat::CMat34d &pJacobian,int pJoint,int pCol)
{
    const double lStep = 0.01;

    mat::CVec3d lNewError;
    
    mJoints[pJoint]+=lStep;
    ComputeErrors(pTarget,lNewError);
    mJoints[pJoint]-=lStep;
    
    pJacobian(0,pCol)=(lNewError.x()-pError.x())/lStep;
    pJacobian(1,pCol)=(lNewError.y()-pError.y())/lStep;
    pJacobian(2,pCol)=(lNewError.z()-pError.z())/lStep;
}

void CHeadPose::ComputeJacobian(mat::CVec3d &pError,
            const mat::CVec3d &pTarget,mat::CMat34d &pJacobian)
{
    ComputeErrors(pTarget,pError);

    ComputeJacobianColumn(pError,pTarget,pJacobian,JOINT_NECK_YAW,0);
    ComputeJacobianColumn(pError,pTarget,pJacobian,JOINT_NECK_ROLL,1);
    ComputeJacobianColumn(pError,pTarget,pJacobian,JOINT_NECK_PITCH,2);
}

void CHeadPose::SolveDeltaAngles(const mat::CVec3d &pError,
                    const mat::CMat34d &pJ,mat::CVec3d &pDelta)
{
    mat::CMat34d lIJ;

    double lDet = pJ(0,0)*(pJ(1,1)*pJ(2,2)-
                  pJ(1,2)*pJ(2,1))+
                  pJ(0,1)*(pJ(1,2)*pJ(2,0)-
                  pJ(1,0)*pJ(2,2))+
                  pJ(0,2)*(pJ(1,0)*pJ(2,1)-
                  pJ(1,1)*pJ(2,0));

    lDet=(lDet==0.0?1.0:1.0/lDet);

    lIJ(0,0) = lDet*(pJ(1,1)*pJ(2,2) - pJ(1,2)*pJ(2,1));
    lIJ(1,0) = lDet*(pJ(1,2)*pJ(2,0) - pJ(1,0)*pJ(2,2));
    lIJ(2,0) = lDet*(pJ(1,0)*pJ(2,1) - pJ(1,1)*pJ(2,0));
    lIJ(0,1) = lDet*(pJ(0,2)*pJ(2,1) - pJ(0,1)*pJ(2,2));
    lIJ(1,1) = lDet*(pJ(0,0)*pJ(2,2) - pJ(0,2)*pJ(2,0));
    lIJ(2,1) = lDet*(pJ(0,1)*pJ(2,0) - pJ(0,0)*pJ(2,1));
    lIJ(0,2) = lDet*(pJ(0,1)*pJ(1,2) - pJ(0,2)*pJ(1,1));
    lIJ(1,2) = lDet*(pJ(0,2)*pJ(1,0) - pJ(0,0)*pJ(1,2));
    lIJ(2,2) = lDet*(pJ(0,0)*pJ(1,1) - pJ(0,1)*pJ(1,0));

    pDelta=lIJ*pError;
}

/*namespace pacohead*/ } /*namespace robot*/ } /*namespace buola*/ }